/*

V/Hz acim motor control

*/

#include <p30F4011.h>
#include "settings.h"
#include "AC-Controller.h"
#include "globals.h"
#include "acim_vhz.h"
#include "eeprom.h"


// V/Hz variables //////////////////////////////////////////////
unsigned int speed_val; //how much to advance in the table for a given Hz
unsigned int hertz; //the speed we'd like to spin the motor
unsigned int voltage; //compensator for voltage. In the range [0,1023] 0 being zero volts, 1023 being full volts

unsigned int positionHiResPH1;  // A circle broken up into 65536 "degrees".  Add 504 each time to positionHiResPH1 if you want to have motor speed be 60 Hz, if it's being updated at 8kHz.
int sinPhase1;

unsigned int positionHiResPH2;
int sinPhase2;


unsigned int positionHiResPH3;
int sinPhase3;

int duty1, duty2, duty3;

//motor set points
int motor_set_volts;
int motor_set_hertz;

void init_vhz(void) {
	motor_set_volts = ee_get_motor_volt();
	motor_set_hertz = ee_get_motor_hertz();
}

void MotorLoop_VHz() {
	/*The boring theory behind this code:
	//throttle_feedback has been conditioned to be within the range [0,1023] which is 10
	//bits of precision. 0 = No throttle 1023 = Full Throttle
	//this routine is called every other timer tick or about 8kHz.
	//so that's 8000 / 60 = 133 runs through this loop for every cycle at 60 Hz
	//The positionHiResXXX variables are ranged [0,65535]
	//That means we need to add (65535 / (8000 / Hz)) to the values of each phase 
	//in order to get the proper speed. We'll go ahead and overrev the motor. Assuming
	//it wanted to go 60Hz then we'll do field weakening and allow for up to 120Mhz which
	//will double speed the motor at expense of the torque dropping off.
	//So, the throttle ranges [0,1023] and the hertz [0,120]. Range linearly
	//However, voltage should linearly taper with throttle too. Max voltage is at
	//512 throttle. Over that the voltage stays constant. Under that it ranges linearly.
	//This scales the PWM values to send out
	//right now the maximum hertz is hard coded but dont do that forever
	*/

	int measured_v = 250; //the voltage we measured on the DC bus. I don;'t have that yet so it is hardcoded.
	long temp;

	hertz = (int)((long)(15 * throttle_feedback) >> 7); //the result is 0 - 120

	speed_val =((long)((long)((long)(65535 << 10) / 8000 * hertz)) >> 10);

	/*
		It's just easier to use a 32 bit variable here since the numbers get so big
	*/
	temp = (long)((long)hertz << 10) / (long)motor_set_hertz;;
	temp = (temp * (long)motor_set_volts) / (long)measured_v;
	voltage = temp;
	if (voltage > 1023) voltage = 1023;
	if (voltage < 102) voltage = 102; //lock the low end at about a tenth

	positionHiResPH1 += speed_val;
	positionHiResPH2 = positionHiResPH1 + 0x5555; // 120 degree offset.  Who cares if it overflows.  It just wraps around.  Heck ya.
	positionHiResPH3 = positionHiResPH1 + 0xAAAA; // 240 degree offset.  Heck ya.  That's what I'm talking about.  This is exciting...
	
	sinPhase1 = _sin[positionHiResPH1 >> 7];  // (positionHiResPH1 >> 7) is in [0, 511].  sinPhase1 is in [-32767, 32767].  32767 corresponds to 1, or so.  crap. pretend that 1 corresponds to 32768.
	sinPhase2 = _sin[positionHiResPH2 >> 7];  // 
	sinPhase3 = _sin[positionHiResPH3 >> 7];

    //sinPhaseX is in [-32768, 32768] which is 15 bits of precision and a sign bit
	//voltage is [0, 1023] which is ten bits of precision.
	//this answer has to be scaled to plus or minus PWM_BIAS [-PWM_BIAS, PWM_BIAS] 
	//so multiply by PWM_BIAS then divide by the previous maximum 1<<25
	//unfortunately the multiply could overflow precision so scale everything down
	//in the code this is done by first shifting 10 right (divide by 1024) then doing
	//the multiply, then shifting the rest of it (15 right).
	//the duty cycle is in the middle to command no real work. Being lower than the middle
	//point makes the power negative, being higher makes it positive
	if (throttle_feedback > 10) {
		duty1 = (((((long)voltage * (long)sinPhase1) >> 10) * PWM_BIAS) >> 15) + PWM_BIAS;
		duty2 = (((((long)voltage * (long)sinPhase2) >> 10) * PWM_BIAS) >> 15) + PWM_BIAS;
		duty3 = (((((long)voltage * (long)sinPhase3) >> 10) * PWM_BIAS) >> 15) + PWM_BIAS;
	}
	else {
		duty1 = 0;
		duty2 = 0;
		duty3 = 0;
	}

	PDC1 = duty1;
	PDC2 = duty2;
	PDC3 = duty3; 

}
